% dg/du, 誤差関数g, 更新誤差関数g~, 状態x を作成する．
function [f_dgdu,f_g_new,f_gtil_new,f_bfx,f_dgdu_new] = make_f_g(f_xnext,f_dxn_dx,f_dxn_du,f_xN_bfu,n,m,N,x0,mu,r_LMO,v_f)


%% f_bfx 状態xの関数
% Return x[N](bfu)
    function [vec_val] = proto_make_f_bfx(bfu)
        bfx = zeros(n*(N+1),1);
        bfx(1:n) = x0;
        for k = 1:N
            bfx(1+k*n:(k+1)*n) = f_xnext(bfx(1+(k-1)*n:k*n),bfu(1+(k-1)*m:k*m));
        end
        vec_val = bfx;
    end
f_bfx = @proto_make_f_bfx;

%% f_g_new 終端状態xfとの誤差関数
% Return g(bfu,x0,xf)
    function [g_new] = proto_make_f_g_new(bfu)% 
        g_new = [
          ((([1, 0, 0, 0]*f_xN_bfu(bfu)) - 1 + mu)^2 + ([0, 1, 0, 0]*f_xN_bfu(bfu))^2-r_LMO^2);
          ([0, 0, 1, 0]*f_xN_bfu(bfu))^2 + ([0, 0, 0, 1]*f_xN_bfu(bfu))^2-v_f^2;
          (([1, 0, 0, 0]*f_xN_bfu(bfu)) - 1 + mu)*([0, 0, 1, 0]*f_xN_bfu(bfu)) + ([0, 1, 0, 0]*f_xN_bfu(bfu))*([0, 0, 0, 1]*f_xN_bfu(bfu))];
    end
f_g_new = @proto_make_f_g_new;

%% f_gtill_new
% gtil_new = f_gtil(bfu(k).v,c,f_xN_bfu(bfu(k).v)); 引数はこの通りに入る．cvxで使用.
% m=2;
% N=2000;
    function [gtil_new] = proto_make_f_gtil_new(bfu,c)% 
        gtil_new = [
        (([1, 0, 0, 0]*f_xN_bfu(bfu) - 1 + mu)^2 + ([0, 1, 0, 0]*f_xN_bfu(bfu))^2-(c*r_LMO^2+(1-c)*((([1, 0, 0, 0]*f_xN_bfu(bfu)) - 1 + mu)^2 + ([0, 1, 0, 0]*f_xN_bfu(bfu))^2)));
         ([0, 0, 1, 0]*f_xN_bfu(bfu))^2 + ([0, 0, 0, 1]*f_xN_bfu(bfu))^2-(c*v_f^2+(1-c)*(([0, 0, 1, 0]*f_xN_bfu(bfu))^2 + ([0, 0, 0, 1]*f_xN_bfu(bfu))^2));
        (([1, 0, 0, 0]*f_xN_bfu(bfu))- 1 + mu)*([0, 0, 1, 0]*f_xN_bfu(bfu)) + ([0, 1, 0, 0]*f_xN_bfu(bfu))*([0, 0, 0, 1]*f_xN_bfu(bfu))-(1-c)*(([1, 0, 0, 0]*f_xN_bfu(bfu))- 1 + mu)*([0, 0, 1, 0]*f_xN_bfu(bfu)) + ([0, 1, 0, 0]*f_xN_bfu(bfu))*([0, 0, 0, 1]*f_xN_bfu(bfu))];
    end
f_gtil_new = @proto_make_f_gtil_new;
%% f_dgdu　誤差関数のu微分⇒誤差関数をl1最適な方向に更新する更新ベクトルwを求める．gを直接微分せずに，fから求まるのでうれしい．要確認
% dgdu = f_dgdu(bfu(k).v); cvxで使用
    function [mat_val] = proto_make_f_dgdu(bfu)% u の関数ととらえる
        bfx = zeros(n*N,1);
        bfx(1:n) = x0;% 上4こ初期値
        for k = 1:N-1
            bfx(1+k*n:(k+1)*n) = f_xnext(bfx(1+(k-1)*n:k*n),bfu(1+(k-1)*m:k*m));% 4nこ埋める．
        end

        list_dfdu =struct();%df/du の構造体を作成．
        for i = 0:N-1
            list_dfdu(i+1).M = f_dxn_du(bfx(1+i*n:(i+1)*n),bfu(1+i*m:(i+1)*m));
        end

        M_dgdu = zeros(n,m*N);% n×mN の零行列 dg/du
        Q_dfdx = eye(n,n);% n×n の単位行列 df/dx
        M_dgdu(1:n,1+(N-1)*m:N*m) = list_dfdu(i+1).M;% 
        
        for i = N-2:-1:0
            Q_dfdx = Q_dfdx*f_dxn_dx(bfx(1+(i+1)*n:(i+2)*n),bfu(1+(i+1)*m:(i+2)*m));% df/dx の行列
            M_dgdu(1:n,1+i*m:(i+1)*m) = Q_dfdx*list_dfdu(i+1).M;% dg/du の正方行列
        end
        
        mat_val = M_dgdu;% 行列 dg/du を作成．
    end
f_dgdu = @proto_make_f_dgdu;

%% f_dgdu_new
% Return g(bfu,x0,xf)
    function [dgdu_new] = proto_make_f_dgdu_new(bfu)% 
        dgdu_new = [
        ((2*(([1, 0, 0, 0]*f_xN_bfu(bfu)) - 1 + mu))*([1, 0, 0, 0]*f_dgdu(bfu)) + 2*([0, 1, 0, 0]*f_xN_bfu(bfu))*([0, 1, 0, 0]*f_dgdu(bfu)));
         (2*([0, 0, 1, 0]*f_xN_bfu(bfu)))*([0, 0, 1, 0]*f_dgdu(bfu)) + (2*([0, 0, 0, 1]*f_xN_bfu(bfu)))*([0, 0, 0, 1]*f_dgdu(bfu));
        ([1, 0, 0, 0]*f_dgdu(bfu))*([0, 0, 1, 0]*f_xN_bfu(bfu)) + (([1, 0, 0, 0]*f_xN_bfu(bfu)) - 1 + mu)*([0, 0, 1, 0]*f_dgdu(bfu))+ ([0, 1, 0, 0]*f_dgdu(bfu))*([0, 0, 0, 1]*f_xN_bfu(bfu)) + ([0, 1, 0, 0]*f_xN_bfu(bfu))*([0, 0, 0, 1]*f_dgdu(bfu))];
    end
f_dgdu_new = @proto_make_f_dgdu_new;

end